# Python 3.4.4 (v3.4.4:737efcadf5a6, Dec 20 2015, 19:28:18) [MSC v.1600 32 bit (Intel)] on win32
# 
import numpy as np
import math
import time						# Required for calculating the progress
from progress.bar import Bar 	# Required for calculating the progress
from sklearn.metrics import mean_squared_error
import matplotlib.pyplot as plt

# FUNCTIONS:
from functions import createRefSignal, fnProcessModel, fnPlotResults, getProcessParameters

# ALLOCATION:
from allocation import *

# CLASSES:
from Clouds import CloudsRECCo
from Evolve import EvolveRECCo
from Adapt  import AdaptRECCo
from openpyxl import Workbook

print("Initializating...")

#------------------------------------------------------------------------------------------
# CHOOSE TYPE OF THE PROCESS:
	# PROCESS_ID = 1 --> artificial first order process with quadratic static nonlinearity
	# PROCESS_ID = 2 --> hydraulic pilot plant of two tanks
	# PROCESS_ID = 3 --> yourself
PROCESS_ID = 3;
U_MIN, U_MAX, Y_MIN, Y_MAX, TAU, T_S, A_R = getProcessParameters(PROCESS_ID) 
# This function loads the process parameters
#------------------------------------------------------------------------------------------
if PROCESS_ID==1:
	print("----------Chosen Process: Artificial first order process with quadratic static nonlinearity----------")
if PROCESS_ID==2:
	print("----------Chosen Process: Hydraulic pilot plant of two tanks----------")
if PROCESS_ID==3:
	print("----------Chosen Process: Yourself----------")

#------------------------------------------------------------------------------------------
# CHOOSE NUMBER OF SEQUENCES:
	# Define how many times the reference sequence is repeated
NUM_SEQ = 2
#------------------------------------------------------------------------------------------


#------------------------------------------------------------------------------------------
# ADAPTIVE LAW PARAMETERS:
D_DEAD     = (Y_MAX-Y_MIN)*0.005 			 # dead time to stop adaptation (0.5% of output range)
ALPHA      = (U_MAX-U_MIN)*0.005  			 # adaptation gain (0.5% of input range)
ALPHA_LIST = ALPHA*np.ones((4)) 			 # vector of adaptation gains
print(ALPHA_LIST)
# CREATE REFERENCE SIGNAL 
arrRef = createRefSignal(Y_MIN, Y_MAX, TAU)
lenSim = len(arrRef)

# DEFINE CLASSES
myEvolve = EvolveRECCo(2)
myAdapt  = AdaptRECCo(4, [D_DEAD, ALPHA_LIST, U_MIN, U_MAX])
#------------------------------------------------------------------------------------------

y_r  = Y_MIN
y    = Y_MIN

print("Start...")
bar = Bar("Processing", max=NUM_SEQ*lenSim-lenSim-NUM_SEQ+1)

for iidx in range(1,NUM_SEQ):
	for idx in range(1, lenSim):
		bar.next()
		idxK = idxK + 1
		r    = arrRef[idx]
		
		# Calculate error:
		y_r   = A_R*y_r + (1-A_R)*r
		dEps  = (y_r - y) - eps
		if (u!=U_MIN or u!=U_MAX): iEps = iEps + eps   # Anti wind-up mechanism
		err   = r - y
		eps   = y_r - float(y)
		
		# --------------------------------------
		# Evolving mechanism:
		# --------------------------------------
		data = np.array([eps/((Y_MAX-Y_MIN)/2), (y_r-Y_MIN)/(Y_MAX-Y_MIN)])
		myEvolve.addPoint(data, idxK) 
		meberList    = myEvolve.memberVector
		nClouds      = len(meberList)
		statusEvolve = myEvolve.status
		
		# --------------------------------------
		# Adaptive law:
		# --------------------------------------
		inputList = [float(err), float(eps), float(iEps), float(dEps), float(r)]
		myAdapt.fnAdaptiveLaw(inputList, meberList, statusEvolve, idxK) 
		u = myAdapt.u
		
		# --------------------------------------
		# Control the process (model):
		# --------------------------------------
		y = fnProcessModel(PROCESS_ID, u, y, idxK)


		# Stire the variables in buffers:
		buff_Y.append(y)
		buff_U.append(u)
		buff_Y_R.append(y_r)
		buff_R.append(r)
		buff_EPS.append(eps)
		buff_ERR.append(-err)
		buff_Data  = np.vstack([buff_Data, data])
		if np.mod(idxK, 4)==0 or myEvolve.status == 1:
			if myEvolve.status == 1 and idxK>10 : # New cloud is added
				buff_Theta = np.hstack([buff_Theta, np.zeros((buff_Theta.shape[0],4))])

			buff_Theta = np.vstack([buff_Theta, myAdapt.Theta.reshape(1,myAdapt.Theta.size)])
		#print(buff_Theta.shape)
		
#plt.plot(buff_Y[1:lenSim],label="Output",color='green')
#plt.plot(buff_R[1:lenSim],label="Reference",color='blue')
#plt.plot(buff_ERR[1:lenSim],label="Error",color='red')

# Call function to plot the results
fnPlotResults(lenSim, idxK, buff_Y, buff_U, buff_Y_R, buff_R, buff_EPS, buff_Data, buff_Theta, myEvolve)

# 第几个d开始err小于0.001
for i in range(len(buff_ERR)):
	if buff_ERR[i] < 0.001:
		print('The %d time converges.' % (i + 1))
		break

# 保存第19个周期的err
wb = Workbook()
ws = wb.create_sheet()
ws.append(buff_ERR[628*18:628*18+628])
wb.save('the_19th_err.xlsx')
wb.close()

# 保存第19个周期的u
wb = Workbook()
ws = wb.create_sheet()
ws.append(buff_U[628*18:628*18+628])
wb.save('the_19th_u.xlsx')
wb.close()

# err的std和RMSE
std_err = np.std(np.array(buff_ERR), ddof=1)
rmse_err = np.sqrt(mean_squared_error(buff_Y, buff_R))
print('The std is %lf, the RMSE is %lf' % (std_err, rmse_err))

print("\nEnd...")

#bar.finish()

